/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-12-18     MBW       the first version
 */
#include "xunji_task.h"
#include "PPM.h"
#include "data_define.h"
#include "motor.h"
void xunji_task(void *p)
{
    float errx,erry;
    while(1)
    {
        if(channel_6<900){
            control_data.set_position.x = PPM_con.PPM_pitch;
            control_data.set_position.y = PPM_con.PPM_roll;
        }
        else {
            control_data.set_position.x = 50;
            control_data.set_position.y = 200;
            errx = fabs(control_data.set_position.x-control_data.get_position.x);
            erry = fabs(control_data.set_position.y-control_data.get_position.y);
            if(errx<3 && erry<3)  easyblink(led1, 4, 50, 200);

        }

        rt_thread_mdelay(10);
    }
}
void xunji_task_init(void)
{
    rt_thread_t tid1 = RT_NULL;
    tid1 = rt_thread_create("xunji_task",xunji_task, RT_NULL, 2000, 16, 20);
    if (tid1 != RT_NULL)rt_thread_startup(tid1),rt_kprintf("...xunji_task is ok!\n");
}
INIT_APP_EXPORT(xunji_task_init);
